#include "OneButton.h"
#include <esp_now.h>
#include <WiFi.h>
#include <esp_wifi.h>  // only for esp_wifi_set_channel()
//启用ESPNow后，ADC2管教无法使用，即（4，0，2，15，13，12，14，27，25，26）
#define CHANNEL 1
#define PRINTSCANRESULTS 0
#define DELETEBEFOREPAIR 0


#define PIN_INPUT_LEFT 18
#define PIN_INPUT_RIGHT 19
#define PIN_INPUT_1 21
#define PIN_INPUT_2 22
#define PIN_INPUT_3 23
#define PIN_LED 2


#define ADC_IO_LEFT_1 32
#define ADC_IO_LEFT_2 33
#define ADC_IO_RIGHT_1 34
#define ADC_IO_RIGHT_2 35


// 设置数据结构体
typedef struct struct_message {
  int left_1;
  int left_2;
  int right_1;
  int right_2;
  int btn_1;
  int btn_2;

} struct_message;

struct_message cmdData;



uint16_t rocker_left_1 = 0;
uint16_t rocker_left_2 = 0;
uint16_t rocker_right_1 = 0;
uint16_t rocker_right_2 = 0;

OneButton button_left(PIN_INPUT_LEFT, true);
OneButton button_right(PIN_INPUT_RIGHT, true);

esp_now_peer_info_t slave;
unsigned long pressStartTime_left;
unsigned long pressStartTime_right;

int ledState = LOW;

void sendRCData(int left_1, int left_2, int right_1, int right_2, int btn_1, int btn_2) {

  cmdData.left_1 = left_1;
  cmdData.left_2 = left_2;
  cmdData.right_1 = right_1;
  cmdData.right_2 = right_2;
  cmdData.btn_1 = btn_1;
  cmdData.btn_2 = btn_2;

  if (slave.channel == CHANNEL) {
    bool isPaired = manageSlave();
    if (isPaired) {
      const unsigned char *peer_addr = slave.peer_addr;
      esp_err_t result = esp_now_send(peer_addr, (uint8_t *)&cmdData, sizeof(cmdData));
      Serial.print("Send Status: ");
      if (result == ESP_OK) {
        Serial.println("Success");
      } else if (result == ESP_ERR_ESPNOW_NOT_INIT) {
        Serial.println("ESPNOW not Init.");
      } else if (result == ESP_ERR_ESPNOW_ARG) {
        Serial.println("Invalid Argument");
      } else if (result == ESP_ERR_ESPNOW_INTERNAL) {
        Serial.println("Internal Error");
      } else if (result == ESP_ERR_ESPNOW_NO_MEM) {
        Serial.println("ESP_ERR_ESPNOW_NO_MEM");
      } else if (result == ESP_ERR_ESPNOW_NOT_FOUND) {
        Serial.println("Peer not found.");
      } else {
        Serial.println("Not sure what happened");
      }
    } else {
      Serial.println("Slave pair failed!");
    }
  }
}



void checkTicks() {
  button_left.tick();
  button_right.tick();
}
int btn_1_value = 0;
int btn_2_value = 0;
//单击
void singleClick_left() {
  Serial.println("singleClick_left() detected.");
  if (btn_1_value < 100) {
    btn_1_value++;
  } else {
    btn_1_value = 1;
  }
}
//单击
void singleClick_right() {
  Serial.println("singleClick_right() detected.");
  if (btn_2_value < 100) {
    btn_2_value++;
  } else {
    btn_2_value = 1;
  }
}

//双击
void doubleClick_left() {
  Serial.println("doubleClick_left() detected.");

  ledState = !ledState;  // reverse the LED
  digitalWrite(PIN_LED, ledState);
}
//双击
void doubleClick_right() {
  Serial.println("doubleClick_right() detected.");

  ledState = !ledState;  // reverse the LED
  digitalWrite(PIN_LED, ledState);
}



//长按开始
void pressStart_left() {
  Serial.println("pressStart_left()");
  pressStartTime_left = millis() - 1000;  // as set in setPressTicks()
}
//长按开始
void pressStart_right() {
  Serial.println("pressStart_right()");
  pressStartTime_right = millis() - 1000;  // as set in setPressTicks()
}

//长按结束
void pressStop_left() {
  Serial.print("pressStop_left(");
  Serial.print(millis() - pressStartTime_left);
  Serial.println(") detected.");
}
//长按结束
void pressStop_right() {
  Serial.print("pressStop_right(");
  Serial.print(millis() - pressStartTime_right);
  Serial.println(") detected.");
}

//更新摇杆读数
void updateADCValue() {
  rocker_left_1 = analogRead(ADC_IO_LEFT_1);
  rocker_left_2 = analogRead(ADC_IO_LEFT_2);
  rocker_right_1 = analogRead(ADC_IO_RIGHT_1);
  rocker_right_2 = analogRead(ADC_IO_RIGHT_2);

  // Serial.print("  left_1: ");
  // Serial.println(rocker_left_1);
  // Serial.print("  left_2: ");
  // Serial.println(rocker_left_2);
  // Serial.print("  right_1: ");
  // Serial.println(rocker_right_1);
  // Serial.print("  right_2: ");
  // Serial.println(rocker_right_2);
  // Serial.println("***************************");
}


//解析摇杆读数
void parseADCValue() {
}

// Init ESP Now with fallback
void InitESPNow() {
  WiFi.disconnect();
  if (esp_now_init() == ESP_OK) {
    Serial.println("ESPNow Init Success");
  } else {
    Serial.println("ESPNow Init Failed");
    ESP.restart();
  }
}

// Scan for slaves in AP mode
void ScanForSlave() {
  int16_t scanResults = WiFi.scanNetworks(false, false, false, 300, CHANNEL);  // Scan only on one channel
  // reset on each scan
  bool slaveFound = 0;
  memset(&slave, 0, sizeof(slave));

  Serial.println("");
  if (scanResults == 0) {
    Serial.println("No WiFi devices in AP Mode found");
  } else {
    // Serial.print("Found ");
    // Serial.print(scanResults);
    // Serial.println(" devices ");
    for (int i = 0; i < scanResults; ++i) {
      // Print SSID and RSSI for each device found
      String SSID = WiFi.SSID(i);
      int32_t RSSI = WiFi.RSSI(i);
      String BSSIDstr = WiFi.BSSIDstr(i);


      delay(10);
      // Check if the current device starts with `Slave`
      if (SSID.indexOf("Slave") == 0) {
        // Get BSSID => Mac Address of the Slave
        int mac[6];
        if (6 == sscanf(BSSIDstr.c_str(), "%x:%x:%x:%x:%x:%x", &mac[0], &mac[1], &mac[2], &mac[3], &mac[4], &mac[5])) {
          for (int ii = 0; ii < 6; ++ii) {
            slave.peer_addr[ii] = (uint8_t)mac[ii];
          }
        }

        slave.channel = CHANNEL;  // pick a channel
        slave.encrypt = 0;        // no encryption

        slaveFound = 1;
        // we are planning to have only one slave in this example;
        // Hence, break after we find one, to be a bit efficient
        break;
      }
    }
  }

  if (slaveFound) {
    Serial.println("Slave Found, processing..");
  } else {
    Serial.println("Slave Not Found, trying again.");
  }

  // clean up ram
  WiFi.scanDelete();
}

// Check if the slave is already paired with the master.
// If not, pair the slave with master
bool manageSlave() {
  if (slave.channel == CHANNEL) {
    if (DELETEBEFOREPAIR) {
      deletePeer();
    }

    bool exists = esp_now_is_peer_exist(slave.peer_addr);
    if (exists) {
      // Slave already paired.
      Serial.println("Already Paired");
      return true;
    } else {
      // Slave not paired, attempt pair
      esp_err_t addStatus = esp_now_add_peer(&slave);
      if (addStatus == ESP_OK) {
        // Pair success
        Serial.println("Pair success");
        return true;
      } else if (addStatus == ESP_ERR_ESPNOW_NOT_INIT) {
        // How did we get so far!!
        Serial.println("ESPNOW Not Init");
        return false;
      } else if (addStatus == ESP_ERR_ESPNOW_ARG) {
        Serial.println("Invalid Argument");
        return false;
      } else if (addStatus == ESP_ERR_ESPNOW_FULL) {
        Serial.println("Peer list full");
        return false;
      } else if (addStatus == ESP_ERR_ESPNOW_NO_MEM) {
        Serial.println("Out of memory");
        return false;
      } else if (addStatus == ESP_ERR_ESPNOW_EXIST) {
        Serial.println("Peer Exists");
        return true;
      } else {
        Serial.println("Not sure what happened");
        return false;
      }
    }
  } else {
    // No slave found to process
    Serial.println("No Slave found to process");
    return false;
  }
}

void deletePeer() {
  esp_err_t delStatus = esp_now_del_peer(slave.peer_addr);
  Serial.print("Slave Delete Status: ");
  if (delStatus == ESP_OK) {
    // Delete success
    Serial.println("Success");
  } else if (delStatus == ESP_ERR_ESPNOW_NOT_INIT) {
    // How did we get so far!!
    Serial.println("ESPNOW Not Init");
  } else if (delStatus == ESP_ERR_ESPNOW_ARG) {
    Serial.println("Invalid Argument");
  } else if (delStatus == ESP_ERR_ESPNOW_NOT_FOUND) {
    Serial.println("Peer not found.");
  } else {
    Serial.println("Not sure what happened");
  }
}


void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status) {
  // char macStr[18];
  // snprintf(macStr, sizeof(macStr), "%02x:%02x:%02x:%02x:%02x:%02x",
  //          mac_addr[0], mac_addr[1], mac_addr[2], mac_addr[3], mac_addr[4], mac_addr[5]);
  // Serial.print("Last Packet Sent to: ");
  // Serial.println(macStr);
  // Serial.print("Last Packet Send Status: ");
  // Serial.println(status == ESP_NOW_SEND_SUCCESS ? "Delivery Success" : "Delivery Fail");
}
void setup() {
  Serial.begin(115200);
  Serial.println("One Button Example with interrupts.");

  pinMode(PIN_LED, OUTPUT);  // sets the digital pin as output
  digitalWrite(PIN_LED, ledState);

  analogReadResolution(12);  //设置ADC的分辨率为12位(0-4096)

  attachInterrupt(digitalPinToInterrupt(PIN_INPUT_LEFT), checkTicks, CHANGE);
  attachInterrupt(digitalPinToInterrupt(PIN_INPUT_RIGHT), checkTicks, CHANGE);

  button_left.attachClick(singleClick_left);
  button_left.attachDoubleClick(doubleClick_left);
  button_left.setPressTicks(1000);  // that is the time when LongpressStart_left is called
  button_left.attachLongPressStart(pressStart_left);
  button_left.attachLongPressStop(pressStop_left);


  button_right.attachClick(singleClick_right);
  button_right.attachDoubleClick(doubleClick_right);
  button_right.setPressTicks(1000);  // that is the time when LongpressStart_left is called
  button_right.attachLongPressStart(pressStart_left);
  button_right.attachLongPressStop(pressStop_right);

  WiFi.mode(WIFI_STA);
  esp_wifi_set_channel(CHANNEL, WIFI_SECOND_CHAN_NONE);
  InitESPNow();
  esp_now_register_send_cb(OnDataSent);

  ScanForSlave();
}

void loop() {
  button_left.tick();
  button_right.tick();


  delay(50);
  updateADCValue();
  parseADCValue();


  if (slave.channel == CHANNEL) {

    bool isPaired = manageSlave();
    if (isPaired) {
      sendRCData(rocker_left_1, rocker_left_2, rocker_right_1, rocker_right_2, btn_1_value, btn_2_value);
    } else {
      Serial.println("Slave pair failed!");
    }
  } else {
    ScanForSlave();
  }
}
